#ifndef __DALELIB_SE3_H__
#define __DALELIB_SE3_H__

#include <math.h>
#include <dalelib/maths/v3.h>
#include <dalelib/maths/quat.h>
#include <dalelib/maths/m4.h>

namespace dalelib
{

// *** Special Euclidean Lie group ***
class se3
{
private:

	quat _rot;
	v3 _pos;

public:

	se3() { }
	se3( const quat _Rot, const v3 _Pos ) : _rot(_Rot), _pos(_Pos) { }

	static const se3 identity;

	const quat& rot() const { return _rot; }
	      quat& rot()       { return _rot; }
	const v3& pos() const { return _pos; }
	      v3& pos()       { return _pos; }

	const se3 operator =(const se3& _Value) { _rot = _Value.rot(); _pos = _Value.pos(); return *this; }
//	const v3 operator *(const v3& _Value) const { return _rot*_Value + _pos; }

	void print(const std::string _Str) const;

	m4 toMatrix()
	{
		return m4(
			_rot[0], _rot[1], _rot[2], 0.f,
			_rot[3], _rot[4], _rot[5], 0.f,
			_rot[6], _rot[7], _rot[8], 0.f,
			_pos[0], _pos[1], _pos[2], 1.f);
	}


};

} // namespace dalelib

#endif
